#include <stdio.h>
/*************************************************************/
/*MRPT INCLUDES */
/*************************************************************/
#include <mrpt/poses/CPose3D.h>
#include <mrpt/hwdrivers/C2DRangeFinderAbstract.h>
#include <mrpt/utils.h>
#include <mrpt/slam.h>
#include <mrpt/gui.h>
#include <mrpt/gui/WxSubsystem.h>
#include <mrpt/gui/WxUtils.h>

#include "CQuadrotor.h"

#include "Quad3DModel.h"

using namespace mrpt;
using namespace mrpt::hwdrivers;
using namespace mrpt::slam;
using namespace mrpt::opengl;
using namespace mrpt::gui;
using namespace mrpt::math;
using namespace mrpt::utils;

using namespace std;
/*************************************************************/

#define NB_MAX_POINTS_SCAN_TELEMETRE 1200
typedef struct {
	unsigned short timeStamp;
	short angleDebut; //*100 deg;
	short angleFin; //*100 deg;
	short pasAngulaire; //*1000 deg
	short nbPointsScan;
	short tabScan[NB_MAX_POINTS_SCAN_TELEMETRE]; // en mm
} TypePacketUDPScanTelemetre;

/*************************************************************/

double scalarProd(TPoint3D p1, TPoint3D p2) {
	return p1[0] * p2[0] + p1[1] * p2[1] + p1[2] * p2[2];
}

int main(int argc, char* argv[]) {

	CQuadrotor quad;
	//quad.pilot.generateHuitWpts();
	bool auto_on = false;

	mrpt::utils::CFileGZOutputStream outs;
	long decimation_count = 0;
	long decimation = 1;
	CPose2D lastOdo;
	TTimeStamp tim_now;

	if (!outs.open("simu.rawlog")) {

		printf("Cannot open output rawlog file.\n");
	}

	double phi_c = 0.0, theta_c = 0.0, psi_c = 0.0, h_c = 2.0;

	bool start_simu = false;

	int aff_counter = 0;

	CDisplayWindowPlotsPtr winPlot;

	std::vector<float> x_toplot;
	std::vector<float> y_toplot;
	
	std::vector<float> x_toplot2;
	std::vector<float> y_toplot2;
	std::vector<float> x_toplot3;
	std::vector<float> y_toplot3;
	
	winPlot = CDisplayWindowPlots::Create("Test_Affichage_Tele_Plot", 600, 500);
	winPlot->axis(-6.0, 6.0, -6.0, 6.0, true);

	CDisplayWindow3DPtr win3D;
	CObservation2DRangeScan temp_obs;

	win3D = CDisplayWindow3D::Create("Simulation", 900, 500);
	C3DWindowDialog* hevt = (C3DWindowDialog*) win3D->getWxObject();
	//hevt->Connect(hevt->GetId(),wxEVT_KEY_DOWN,(wxObjectEventFunction)&CEvtH::OnKeyDown);
	//hevt->Connect(hevt->GetId(),wxEVT_KEY_DOWN,(wxObjectEventFunction)&CEvtH::OnKeyDown);


	win3D->setCameraZoom(20);
	win3D->setCameraAzimuthDeg(-45);

	COpenGLScenePtr scene = COpenGLScene::Create();

	COpenGLViewportPtr view = scene->getViewport("main");

	COpenGLViewportPtr view_map = scene->createViewport("mini-map");
	view_map->setBorderSize(2);
	view_map->setViewportPosition(0.01, 0.01, 0.35, 0.35);
	view_map->setTransparent(false);
	mrpt::opengl::CCamera &cam = view_map->getCamera();
	cam.setAzimuthDegrees(-90);
	cam.setElevationDegrees(90);
	cam.setPointingAt(CPose3D(0, 0, 0));
	cam.setZoomDistance(20);
	cam.setOrthogonal();

	mrpt::opengl::CGridPlaneXYPtr groundPlane =
			mrpt::opengl::CGridPlaneXY::Create(-20, 20, -20, 20, 0, 0.5);
	groundPlane->setColor(0.4, 0.4, 0.4);

	opengl::CCylinderPtr pot1 = opengl::CCylinder::Create(0.2, 0.2, 4, 10, 10);
	opengl::CCylinderPtr pot2 = opengl::CCylinder::Create(0.2, 0.2, 4, 10, 10);
	opengl::CCylinderPtr pot3 = opengl::CCylinder::Create(0.2, 0.2, 4, 10, 10);
	opengl::CCylinderPtr pot4 = opengl::CCylinder::Create(0.2, 0.2, 4, 10, 10);

	opengl::CSetOfObjectsPtr quad_render = opengl::stock_objects::Quad3DModel();

	opengl::CArrowPtr lasert1 = opengl::CArrow::Create(0, 0, 0, 1, 1, 1);
	lasert1->setColor(1, 0, 0);
	opengl::CArrowPtr lasert2 = opengl::CArrow::Create(0, 0, 0, 1, 1, 0);
	lasert2->setColor(0, 1, 0);
	opengl::CArrowPtr lasert3 = opengl::CArrow::Create(0, 0, 0, 1, 0, 1);
	lasert3->setColor(0, 0, 1);

	opengl::CPointCloudPtr render_scan = opengl::CPointCloud::Create();
	render_scan->setColor(0, 0, 1);
	render_scan->setPointSize(5);

	quad.laser.point_cloud = render_scan;

	opengl::CSpherePtr next_wpt_render = opengl::CSphere::Create(0.5);
	next_wpt_render->setColor(0, 1, 0, 0.3);
/*
	pot1->setLocation(-5 * cos(-45 * M_PI / 180.0)-2, 5 * sin(-45 * M_PI
			/ 180.0), 0);
	pot2->setLocation(5 * cos(-45 * M_PI / 180.0)-2, -5 * sin(-45 * M_PI
			/ 180.0), 0);
	pot3->setLocation(-2, 3, 0);
*/
	pot1->setLocation(-5, 0, 0);
	pot2->setLocation(5, 0, 0);
	pot3->setLocation(3, 3, 0);
	pot4->setLocation(-3,-3,0);

	opengl::CBoxPtr box = opengl::CBox::Create(TPoint3D(0, 0, 0), TPoint3D(3,
			3, 3));
	box->setPose(CPose3D(0, -7, 0, 20 * M_PI / 180.0, 20 * M_PI / 180.0, 0));

	/*
	 for(int i=0;i<4;i++)
	 {
	 opengl::CBoxPtr box1 = opengl::CBox::Create(TPoint3D(0,0,0),TPoint3D(3,3,3));
	 box1->setPose(CPose3D(i*3,-7,0,20*M_PI/180.0,0,0));
	 view->insert(box1);
	 
	 opengl::CBoxPtr box2 = opengl::CBox::Create(TPoint3D(0,0,0),TPoint3D(3,3,3));
	 box2->setPose(CPose3D(i*3,4,0,20*M_PI/180.0,0,0));
	 view->insert(box2);
	 }
	 */

	pot1->setName("P1");
	pot2->setName("P2");
	pot3->setName("P3");
	pot4->setName("P4");

	view->insert(groundPlane);
	view->insert(pot1);
	view->insert(pot2);
	view->insert(pot3);
	view->insert(pot4);

	view->insert(quad_render);
	view->insert(render_scan);
	view->insert(lasert1);
	view->insert(lasert2);
	view->insert(lasert3);
	view->insert(next_wpt_render);
	view->insert(box);

	view_map->insert(CRenderizablePtr(groundPlane)); // A copy


	view_map->insert(pot1);
	view_map->insert(pot2);
	view_map->insert(quad_render);

	quad.setView(view);

	char c;

	phi_c = 0.0;
	theta_c = 0.0;

	while (1) {
		//	 phi_c  = 0.0;
		//   theta_c = 0.0;


		opengl::COpenGLScenePtr &ptrScene = win3D->get3DSceneAndLock();

		/*************************************************************************************************/
		/*************************************************************************************************/
		/*  Gestion des evenements claviers */
		/*************************************************************************************************/
		/*************************************************************************************************/

		if (win3D->keyHit()) {

			c = win3D->getPushedKey(NULL);
			//printf("k%i\n",c);

			if (c == 0x20) {
				//TOUCHE SPACE
				if (start_simu) {
					quad.model.is_realtime = false;
					start_simu = false;

				} else {
					quad.model.first_exec = true;
					quad.model.is_realtime = true;
					start_simu = true;
				}
			}
			if (c == 97) {
				// TOUCHE A
				h_c += 0.2;
			}

			if (c == 113) {
				// TOUCHE Q
				h_c -= 0.2;
			}

			if (c == 112) {
				// TOUCHE P
				quad.init();
			}

			if (c == 114) {
				// TOUCHE R
				theta_c = 0;
				phi_c = 0;

			}

			if (c == 116) {
				// TOUCHE T
				if (auto_on)
					auto_on = false;
				else
					auto_on = true;
			}

			if (c == 59) {
				//printf("UP\n");
				theta_c = -8 * M_PI / 180;
			}

			if (c == 61) {
				//printf("DOWN\n");
				theta_c = 8 * M_PI / 180;
			}

			if (c == 58) {
				// printf("LEFT\n");
				phi_c = -8 * M_PI / 180;
			}

			if (c == 60) {
				// printf("RIGHT\n");
				phi_c = 8 * M_PI / 180;
			}

			if (c == 119) {
				// printf("W\n");
				psi_c += -10 * M_PI / 180;
			}

			if (c == 120) {
				// printf("X\n");
				psi_c += 10 * M_PI / 180;
			}

			if (c == 27)
				break;
		}
		/*************************************************************************************************/
		/*************************************************************************************************/
		/*  Affichage d'informations de débug */
		/*************************************************************************************************/
		/*************************************************************************************************/

		if (start_simu) {

			if (auto_on) {

				next_wpt_render->setVisibility(true);
				//TPoint3D wpt = quad.pilot.getCurrentWpt();
				//next_wpt_render->setLocation(wpt.x,-wpt.y,2);
			} else {
				next_wpt_render->setVisibility(false);
			}

			quad.simulate();

			// Mise à jour de l'affichage
			quad_render->setPose(quad.getRenderPose());

			float scan_angle;
			float x0, y0, z0, x1, y1, z1;
			float xb, yb;
			CPose3D ref_pose(quad.getRenderPose());

			x0 = ref_pose[0];
			y0 = ref_pose[1];
			z0 = ref_pose[2];
			scan_angle = -120;
			xb = cos(scan_angle * M_PI / 180);
			yb = sin(scan_angle * M_PI / 180);

			x1 = ref_pose.m_ROT(0, 0) * xb + ref_pose.m_ROT(0, 1) * yb + x0;
			y1 = ref_pose.m_ROT(1, 0) * xb + ref_pose.m_ROT(1, 1) * yb + y0;
			z1 = ref_pose.m_ROT(2, 0) * xb + ref_pose.m_ROT(2, 1) * yb + z0;

			lasert1->setArrowEnds(x0, y0, z0, x1, y1, z1);
			scan_angle = 0;
			xb = cos(scan_angle * M_PI / 180);
			yb = sin(scan_angle * M_PI / 180);

			x1 = ref_pose.m_ROT(0, 0) * xb + ref_pose.m_ROT(0, 1) * yb + x0;
			y1 = ref_pose.m_ROT(1, 0) * xb + ref_pose.m_ROT(1, 1) * yb + y0;
			z1 = ref_pose.m_ROT(2, 0) * xb + ref_pose.m_ROT(2, 1) * yb + z0;

			lasert2->setArrowEnds(x0, y0, z0, x1, y1, z1);

			scan_angle = 120;

			xb = cos(scan_angle * M_PI / 180);
			yb = sin(scan_angle * M_PI / 180);

			x1 = ref_pose.m_ROT(0, 0) * xb + ref_pose.m_ROT(0, 1) * yb + x0;
			y1 = ref_pose.m_ROT(1, 0) * xb + ref_pose.m_ROT(1, 1) * yb + y0;
			z1 = ref_pose.m_ROT(2, 0) * xb + ref_pose.m_ROT(2, 1) * yb + z0;

			lasert3->setArrowEnds(x0, y0, z0, x1, y1, z1);

			/*************************************************************************************************/
			/*************************************************************************************************/
			/* Affichage du Scan Laser */
			/*************************************************************************************************/
			/*************************************************************************************************/

			for (int i = 0; i < 481; i++) {
				double cur_angle = (-120 + i * 0.5) * M_PI / 180.0;
				x_toplot.push_back(quad.laser.scan_obs.scan[i] * cos(cur_angle));
				y_toplot.push_back(quad.laser.scan_obs.scan[i] * sin(cur_angle));
	/*			if(quad.pilot.APloc.analysescan.icp.cart_scan.size()>=481 && quad.pilot.APloc.analysescan.icp.cart_scan_prev.size()>=481)
				{
				  x_toplot.push_back(quad.pilot.APloc.analysescan.icp.cart_scan_match[i].x);
				  y_toplot.push_back(quad.pilot.APloc.analysescan.icp.cart_scan_match[i].y);
				  
				  x_toplot2.push_back(quad.pilot.APloc.analysescan.icp.cart_scan_prev[i].x);
				  y_toplot2.push_back(quad.pilot.APloc.analysescan.icp.cart_scan_prev[i].y);
				  
				  x_toplot3.push_back(quad.pilot.APloc.analysescan.icp.cart_scan[i].x);
				  y_toplot3.push_back(quad.pilot.APloc.analysescan.icp.cart_scan[i].y);
				  } */
			}
      
			winPlot->plot(x_toplot, y_toplot, "r.3", "Laser Scan");
			winPlot->hold_on();
			
					for (int i = 0; i < quad.pilot.APloc.analysescan.prls.line_deb.size(); i++) {

			  x_toplot2.clear();
			  y_toplot2.clear();
			     x_toplot2.push_back(quad.pilot.APloc.analysescan.prls.line_deb[i].x);
			     x_toplot2.push_back(quad.pilot.APloc.analysescan.prls.line_end[i].x);
			     
			     y_toplot2.push_back(quad.pilot.APloc.analysescan.prls.line_deb[i].y);
			     y_toplot2.push_back(quad.pilot.APloc.analysescan.prls.line_end[i].y);
			     winPlot->plot(x_toplot2, y_toplot2, "g-2", "Laser Scan");
			
			} 
			
			
	/*		if(!quad.pilot.APloc.analysescan.icp.first_exec)
				{
			winPlot->plot(x_toplot2, y_toplot2, "b.3", "Laser Scan");
			winPlot->plot(x_toplot3, y_toplot3, "c.3", "Laser Scan");
			} 
		
			for (int i = 0; i < quad.pilot.APloc.analysescan.icp.matched_id_c.size(); i+=3) {
			 int ic = quad.pilot.APloc.analysescan.icp.matched_id_c[i];
			 int ip= quad.pilot.APloc.analysescan.icp.matched_id_p[i];
			  x_toplot2.clear();
			  y_toplot2.clear();
			     x_toplot2.push_back(quad.pilot.APloc.analysescan.icp.cart_scan_prev[ip].x);
			     x_toplot2.push_back(quad.pilot.APloc.analysescan.icp.cart_scan_match[ic].x);
			     
			     y_toplot2.push_back(quad.pilot.APloc.analysescan.icp.cart_scan_prev[ip].y);
			     y_toplot2.push_back(quad.pilot.APloc.analysescan.icp.cart_scan_match[ic].y);
			     winPlot->plot(x_toplot2, y_toplot2, "g-2", "Laser Scan");
			
			}  */
			
			winPlot->hold_off();
			
			x_toplot.clear();
			y_toplot.clear();
			
			x_toplot2.clear();
			y_toplot2.clear();
			
			x_toplot3.clear();
			y_toplot3.clear();

			/*************************************************************************************************/
			/*************************************************************************************************/
			/* Enregistrement d'un fichier RAWLOG ( fichier input de icp-slam , visualisble avec RawlogViewer ... */
			/*************************************************************************************************/
			/*************************************************************************************************/

			if (auto_on && outs.fileOpenCorrectly() && 0 == (decimation_count++
					% decimation)) {
				// Action: Déplacement en lacet

				tim_now = mrpt::system::now();

				CActionCollection acts;
				CActionRobotMovement2D act;
				CActionRobotMovement2D::TMotionModelOptions opts;

				opts.modelSelection = CActionRobotMovement2D::mmGaussian;

				CPose2D odo_now(0, 0, quad.model.m_pose.yaw());
				CPose2D Aodom = odo_now - lastOdo;
				lastOdo = odo_now;

				act.computeFromOdometry(Aodom, opts);
				act.timestamp = tim_now;
				acts.insert(act);

				outs << acts;

				// Observation:

				temp_obs.timestamp = tim_now;

				mrpt::slam::CSensoryFrame sf;

				sf.insert(CObservation2DRangeScanPtr(
						new CObservation2DRangeScan(quad.laser.scan_obs)));

				outs << sf;

			}
			/*************************************************************************************************/
			/*************************************************************************************************/
			/*  Affichage d'informations de débug */
			/*************************************************************************************************/
			/*************************************************************************************************/

			aff_counter++;
			if (aff_counter == 10) {

				win3D->addTextMessage(0.0, 0.98, quad.model.m_pose.asString(),
						mrpt::utils::TColorf(1.0, 1.0, 1.0), 0,
						MRPT_GLUT_BITMAP_HELVETICA_12);

				char mes[1000];
				sprintf(mes, "%.2f,%.2f,%.2f %f", quad.pilot.getPhiC() * 180
						/ M_PI, quad.pilot.getThetaC() * 180 / M_PI,
						quad.pilot.getPsiC() * 180 / M_PI, h_c);
						
				win3D->addTextMessage(0.0, 0.94, mes, mrpt::utils::TColorf(1.0,
						1.0, 1.0), 1, MRPT_GLUT_BITMAP_HELVETICA_12);
						
				win3D->addTextMessage(0.0, 0.9,
						quad.getRenderPose().asString(), mrpt::utils::TColorf(
								1.0, 1.0, 1.0), 2,
						MRPT_GLUT_BITMAP_HELVETICA_12);

				sprintf(mes, "%.4f,%.4f,%.4f", quad.model.getVx(),
						quad.model.getVy(), quad.model.getVz());
				win3D->addTextMessage(0.0, 0.86, mes, mrpt::utils::TColorf(1.0,
						1.0, 1.0), 3, MRPT_GLUT_BITMAP_HELVETICA_12);

				sprintf(mes, "%.4f,%.4f,%.4f", quad.model.acc[0],
						quad.model.acc[1], quad.model.acc[2]);
				win3D->addTextMessage(0.0, 0.82, mes, mrpt::utils::TColorf(1.0,
						1.0, 1.0), 4, MRPT_GLUT_BITMAP_HELVETICA_12);

				aff_counter = 0;
			}

		} /*** FIN if(start_simu)***/

		/*************************************************************************************************/
		/*************************************************************************************************/
		/*  Mise à Jour de l'affichage 3D */
		/*************************************************************************************************/
		/*************************************************************************************************/

		if (win3D) {

			ptrScene = scene;
			win3D->unlockAccess3DScene();

			win3D->forceRepaint();

			sleep(1);
		}

	}

	if (outs.fileOpenCorrectly()) {
		outs.close();
	}
	return 1;
}

